{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[INFO] [1634205548.807207]: 等待move_base action服务器连接...\n",
      "[INFO] [1634205548.807875]: 已连接导航服务\n",
      "[INFO] [1634205548.809395]: 已连接机械臂控制服务\n",
      "[INFO] [1634205548.810872]: 开始识别......\n",
      "[INFO] [1634205554.739898]: 识别结果为：\n",
      "[INFO] [1634205554.740665]: apple\n",
      "[INFO] [1634205554.741170]: 尝试播放本地音频\n",
      "[INFO] [1634205554.773045]: 本地播放失败，尝试在线合成，请确认网络\n",
      "[INFO] [1634205557.078667]: 尝试导航去坐标点:(1,0,0)\n",
      "[INFO] [1634205572.079728]: 时间超时，导航任务取消。\n",
      "[INFO] [1634205573.085008]: 尝试导航去坐标点:(0,0,0)\n",
      "[INFO] [1634205588.085926]: 时间超时，导航任务取消。\n",
      "[INFO] [1634205588.086709]: 开始识别......\n",
      "[INFO] [1634205590.558167]: 识别结果为：\n",
      "[INFO] [1634205590.558674]: greenorange\n",
      "[INFO] [1634205590.559021]: 开始识别......\n",
      "[INFO] [1634205590.670216]: 识别结果为：\n",
      "[INFO] [1634205590.671135]: greenorange\n",
      "[INFO] [1634205590.671639]: 开始识别......\n",
      "[INFO] [1634205590.901161]: 识别结果为：\n",
      "[INFO] [1634205590.901680]: greenorange\n",
      "[INFO] [1634205590.901991]: 开始识别......\n",
      "[INFO] [1634205591.016517]: 识别结果为：\n",
      "[INFO] [1634205591.016956]: greenorange\n",
      "[INFO] [1634205591.017302]: 开始识别......\n",
      "[INFO] [1634205593.016369]: 识别结果为：\n",
      "[INFO] [1634205593.016893]: greenorange\n",
      "[INFO] [1634205593.017254]: 开始识别......\n",
      "[INFO] [1634205593.128824]: 识别结果为：\n",
      "[INFO] [1634205593.129361]: greenorange\n",
      "[INFO] [1634205593.130001]: 开始识别......\n",
      "[INFO] [1634205593.915935]: 识别结果为：\n",
      "[INFO] [1634205593.916464]: greenorange\n",
      "[INFO] [1634205593.916814]: 开始识别......\n",
      "[INFO] [1634205594.025002]: 识别结果为：\n",
      "[INFO] [1634205594.025495]: greenorange\n",
      "[INFO] [1634205594.025819]: 开始识别......\n",
      "[INFO] [1634205595.426383]: 识别结果为：\n",
      "[INFO] [1634205595.426940]: greenorange\n",
      "[INFO] [1634205595.427299]: 开始识别......\n",
      "[INFO] [1634205595.673484]: 识别结果为：\n",
      "[INFO] [1634205595.673969]: greenorange\n",
      "[INFO] [1634205595.674647]: 开始识别......\n",
      "[INFO] [1634205595.909257]: 识别结果为：\n",
      "[INFO] [1634205595.909778]: apple\n",
      "[INFO] [1634205595.910326]: 尝试播放本地音频\n",
      "[INFO] [1634205597.844427]: 尝试导航去坐标点:(1,0,0)\n",
      "[INFO] [1634205612.845714]: 时间超时，导航任务取消。\n",
      "[INFO] [1634205613.853609]: 尝试导航去坐标点:(0,0,0)\n"
     ]
    },
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-2-0c33189dd168>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m     14\u001b[0m         \u001b[0maikit\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mset_claw\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m90\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     15\u001b[0m         \u001b[0mrospy\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msleep\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 16\u001b[0;31m         \u001b[0maikit\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mnav_to\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     17\u001b[0m     \u001b[0;32melif\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlabel\u001b[0m\u001b[0;34m==\u001b[0m\u001b[0;34m\"gereenorange\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     18\u001b[0m         \u001b[0maikit\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mplay_sound\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m\"青桔\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/home/robot/.local/lib/python2.7/site-packages/aikit_lib/aikit_2wd.pyc\u001b[0m in \u001b[0;36mnav_to\u001b[0;34m(self, x, y, d)\u001b[0m\n\u001b[1;32m    109\u001b[0m         \u001b[0mrospy\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mloginfo\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'尝试导航去坐标点:('\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0mstr\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mx\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0;34m\",\"\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0mstr\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0my\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0;34m','\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0mstr\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0md\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0;34m')'\u001b[0m\u001b[0;34m)\u001b[0m  \u001b[0;31m#提示连接成功\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    110\u001b[0m         \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmove_base\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msend_goal\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mgoal\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;31m#发送导航目标\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 111\u001b[0;31m         \u001b[0mfinished_within_time\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmove_base\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mwait_for_result\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mrospy\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mDuration\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m15\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m  \u001b[0;31m#等待10秒，获取导航结果\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    112\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0mfinished_within_time\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0;31m#如果导航失败\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    113\u001b[0m             \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmove_base\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcancel_goal\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;31m#取消导航\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/simple_action_client.pyc\u001b[0m in \u001b[0;36mwait_for_result\u001b[0;34m(self, timeout)\u001b[0m\n\u001b[1;32m    141\u001b[0m                     \u001b[0mtime_left\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mloop_period\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    142\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 143\u001b[0;31m                 \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdone_condition\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mwait\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtime_left\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mto_sec\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    144\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    145\u001b[0m         \u001b[0;32mreturn\u001b[0m \u001b[0mself\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msimple_state\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0mSimpleGoalState\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mDONE\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;32m/usr/lib/python2.7/threading.pyc\u001b[0m in \u001b[0;36mwait\u001b[0;34m(self, timeout)\u001b[0m\n\u001b[1;32m    357\u001b[0m                         \u001b[0;32mbreak\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    358\u001b[0m                     \u001b[0mdelay\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdelay\u001b[0m \u001b[0;34m*\u001b[0m \u001b[0;36m2\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mremaining\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m.05\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 359\u001b[0;31m                     \u001b[0m_sleep\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdelay\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    360\u001b[0m                 \u001b[0;32mif\u001b[0m \u001b[0;32mnot\u001b[0m \u001b[0mgotit\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    361\u001b[0m                     \u001b[0;32mif\u001b[0m \u001b[0m__debug__\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "source": [
    "import rospy   #ROS\n",
    "import cv2     #机器视觉库\n",
    "from aikit_lib import aikit_2wd #AIKIT库\n",
    "\n",
    "aikit=aikit_2wd.robot()   #实例化\n",
    "\n",
    "while True:\n",
    "    label=aikit.object_label()\n",
    "    if(label==\"apple\"):\n",
    "        aikit.play_sound(\"苹果\")\n",
    "        rospy.sleep(1)\n",
    "        aikit.set_claw(70)\n",
    "        aikit.nav_to(1,0,0) \n",
    "        aikit.set_claw(90)\n",
    "        rospy.sleep(1)\n",
    "        aikit.nav_to(0,0,0) \n",
    "    elif(label==\"greenorange\"):\n",
    "        aikit.play_sound(\"青桔\")\n",
    "        rospy.sleep(1)\n",
    "        aikit.set_claw(70)\n",
    "        aikit.nav_to(2,0,0) \n",
    "        aikit.set_claw(90)\n",
    "        rospy.sleep(1)\n",
    "        aikit.nav_to(0,0,0) \n",
    "    elif(label==\"orange\"):   \n",
    "        aikit.play_sound(\"橙子\")\n",
    "        rospy.sleep(1)\n",
    "        aikit.set_claw(70)\n",
    "        aikit.nav_to(2,-1,-180) \n",
    "        aikit.set_claw(90)\n",
    "        rospy.sleep(1)\n",
    "        aikit.nav_to(0,0,0)         "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
